/* Include */
#include "../APP.h"


/* --------------------------------- Macro --------------------------------- */

/* Choose a serial port as JSON receiver  */
#define JSON_PORT_LOG
// #define JSON_PORT_RS485

/* Print back buffer or not */
// #define JSON_CODE_BUFFER_PRINTBACK
// #define JSON_DECODE_BUFFER_PRINTBACK

/* --------------------------------- Macro --------------------------------- */


/* --------------------------------- JSON CODE --------------------------------- */
/*      JSON buffer define :

        {
            "H":"S",                        // Code head : S [StopAll] , A [Alright] ...
            "L":[1,2,3,4,5,6,7,8],          // L         : Left servos value
            "R":[1,2,3,4,5,6,7,8]           // R         : Right servos value
        }

        {"H":"S","L":[1,2,3,4,5,6,7,8],"R":[1,2,3,4,5,6,7,8]}
        {\"H\":\"S\",\"L\":[1,2,3,4,5,6,7,8],\"R\":[1,2,3,4,5,6,7,8]}

        new: 
        {"H":"S","L":[1,2,3,4,5,6,7,8],"R":[1,2,3,4,5,6,7,8],"T":[1,2,3,4,5]}
*/

/* JSON receive buffer */
String JSONcode;

/* JSON decode buffer */
String JSONcode_Head = "S";
int    JSONcode_Servo_L[8] = {0};
int    JSONcode_Servo_R[8] = {0};
int    JSONcode_Servo_T[5] = {0};

/* --------------------------------- JSON CODE --------------------------------- */


/* --------------------------------- Between tasks --------------------------------- */

/* Message struct */
// struct JSON_Message
// {
//     String Codehead;
//     int    Servo_L[8];
//     int    Servo_R[8];
// }JSON_MSG;
JSON_Message JSON_MSG;
struct JSON_Message* p_JSON_MSG;

/* Queues handle */
xQueueHandle Queue_2_App_GUI;
xQueueHandle Queue_2_App_ServoDriver;

#define Queue_Length 3

/* --------------------------------- Between tasks --------------------------------- */


/* Function header */
void Task_App_JSON(void* pvParameters);
void JSONcode_decode();
void JSONcode_readFromPort();
void JSONdecode_buffer_print();
void JSON_MSG_send_2_Apps();


/* App entrance */
void App_JSON()
{
    /* App init */
    HAL_LOG.println("[App_JSON] Initializing");

    /* Create startup thread */
    xTaskCreate(
        Task_App_JSON,
        "App_JSON",
        4096,
        NULL,
        2,
        NULL
    );
}


/* App task */
void Task_App_JSON(void* pvParameters)
{
    (void) pvParameters;

    /* Wait a while */
    delay(200);
    /* Cleadn JSON code buffer */
    JSONcode = "";

    /* Create queues */
    Queue_2_App_GUI = xQueueCreate(Queue_Length, sizeof(struct JSON_Message*));
    Queue_2_App_ServoDriver = xQueueCreate(Queue_Length, sizeof(struct JSON_Message*));

    /* Check if queue is created */
    if ((Queue_2_App_GUI == NULL) || (Queue_2_App_ServoDriver == NULL)) {
        HAL_LOG.println("[APP_JSON] Queue create failed");
        while (1)
            delay(1000);
    }

    /* Loop */
    while (1)
    {
        JSONcode_readFromPort();
        delay(10);
    }
    
    /* End of App_RS485 */
    HAL_LOG.println("[App_JSON] Exit");
    vTaskDelete(NULL);
}


/* Decode JSON and save to buffer */
void JSONcode_decode()
{
    /* JSON decode setup */
    DynamicJsonDocument doc(2048);

    /* Deserialze JSON */
    DeserializationError error = deserializeJson(doc, JSONcode);
    if ( error ) {
        HAL_LOG.println("[App_JSON] JSON deserialize error");
        return;
    }

    /* Save to buffer */
    JSONcode_Head = doc["H"].as<String>();
    copyArray(doc["L"], JSONcode_Servo_L);
    copyArray(doc["R"], JSONcode_Servo_R);
    copyArray(doc["T"], JSONcode_Servo_T);

    /* Print JSON decode buffer */
    #ifdef JSON_DECODE_BUFFER_PRINTBACK
        JSONdecode_buffer_print();
    #endif

    /* Send message to other apps */
    JSON_MSG_send_2_Apps();
}


/* Read JSON code from serial port */
void JSONcode_readFromPort()
{
    #ifdef JSON_PORT_LOG
        if ( HAL_LOG.available() > 0 )
        {
            /* Clean receive buffer */
            JSONcode = "";

            while ( HAL_LOG.available() > 0 )
            {
                JSONcode += char(HAL_LOG.read());
                delay(2);
            }
            
            /* Decode JSON */
            #ifdef JSON_CODE_BUFFER_PRINTBACK
                HAL_LOG.println(JSONcode);
            #endif
            JSONcode_decode();
        }
    #endif
    #ifdef JSON_PORT_RS485
        #ifndef JSON_PORT_LOG
            if ( HAL_RS485.available() > 0 )
                {
                    /* Clean receive buffer */
                    JSONcode = "";

                    while ( HAL_RS485.available() > 0 )
                    {
                        JSONcode += char(HAL_RS485.read());
                        // delay(2);
                    }
                    
                    /* Decode JSON */
                    #ifdef JSON_CODE_BUFFER_PRINTBACK
                        HAL_RS485.println(JSONcode);
                    #endif
                    JSONcode_decode();
                }
        #endif
    #endif
}


/* Print JSON decode buffer */
void JSONdecode_buffer_print()
{
    #ifdef JSON_PORT_LOG
        int i = 0;

        HAL_LOG.println(JSONcode_Head);

        for (i = 0; i < 8; i++)
        {
            HAL_LOG.print(JSONcode_Servo_L[i]);
            HAL_LOG.print(',');
        }

        HAL_LOG.println();
        
        for (i = 0; i < 8; i++)
        {
            HAL_LOG.print(JSONcode_Servo_R[i]);
            HAL_LOG.print(',');
        }

        HAL_LOG.println();
    #endif

    #ifdef JSON_PORT_RS485
        #ifndef JSON_PORT_LOG
            int i = 0;

            HAL_RS485.println(JSONcode_Head);

            for (i = 0; i < 8; i++)
            {
                HAL_RS485.print(JSONcode_Servo_L[i]);
                HAL_RS485.print(',');
            }

            HAL_RS485.println();
            
            for (i = 0; i < 8; i++)
            {
                HAL_RS485.print(JSONcode_Servo_R[i]);
                HAL_RS485.print(',');
            }

            HAL_RS485.println();
        #endif
    #endif
}


/* Send message to other apps */
void JSON_MSG_send_2_Apps()
{
    /* Copy buffer to message */
    JSON_MSG.Codehead = JSONcode_Head;
    for (int i = 0; i < 8; i++)
    {
        JSON_MSG.Servo_L[i] = JSONcode_Servo_L[i];
        JSON_MSG.Servo_R[i] = JSONcode_Servo_R[i];
    }
    for (int i = 0; i < 5; i++)
    {
        JSON_MSG.Servo_T[i] = JSONcode_Servo_T[i];
    }
    

    p_JSON_MSG = &JSON_MSG;
    /* Send message to queues */
    if (Queue_2_App_GUI != 0) {

        xQueueSend(Queue_2_App_GUI, (void*) &p_JSON_MSG, (TickType_t) 0);

        /* Send to queue with no block */
        // if ( xQueueSend(Queue_2_App_GUI, (void*) &p_JSON_MSG, (TickType_t) 0) != pdPASS) {
            // HAL_LOG.println("[App_JSON] Send queue to App_GUI failed");
        // }
    }
    if (Queue_2_App_ServoDriver != 0) {

        xQueueSend(Queue_2_App_ServoDriver, (void*) &p_JSON_MSG, (TickType_t) 0);

        /* Send to queue with no block */
        // if ( xQueueSend(Queue_2_App_ServoDriver, (void*) &p_JSON_MSG, (TickType_t) 0) != pdPASS ) {
            // HAL_LOG.println("[App_JSON] Send queue to App_ServoDriver failed");
        // }
    }
}
